#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@File         :increment_pid_controller.py
@Description  :
@Time         :2024/05/21 10:00:59
@Author       :Lin Yuheng
@Version      :1.0
'''

class INCREMENT_PID_CONTROLLER:
    def __init__(self):
        self.input = 0.0
        self.pre_input = 0.0
        self.pre2_input = 0.0

        self.increment = 0.0
        self.output = 0.0
        self.pre_output = 0.0

    def get_inc_output(self):
        return self.increment
    
    def set_prev_output(self):
        self.pre_output = self.output

    def get_full_output(self):
        self.output = self.pre_output + self.increment
        self.pre_output = self.output
        return self.output

    def reset(self):
        self.pre_input = 0.0
        self.pre2_input = 0.0
        self.output = 0.0
        self.pre_output = 0.0

    def update_input(self):
        self.pre2_input = self.pre_input
        self.pre_input = self.input

    def increment_pid(self, input, Kp, Ki, Kd):
        self.input = input

        self.increment = (
            Kp * (self.input - self.pre_input)
            + Ki * self.input
            + Kd * (self.input - 2 * self.pre_input + self.pre2_input)
        )

        self.update_input()
